#include "utils.h"
#include <GL/glut.h>
#include <string>
#include <sstream>
#include <math.h>
#include <vector>
#include <iostream>
#include <iomanip>
#include "longutils.h"

//cx, cy = (x,y) of center. Screen coords.
void DrawRect(double cx, double cy, double width)
{
	glBegin(GL_QUADS);
	
	glVertex2d(cx-width, cy-width);
	glVertex2d(cx-width, cy+width);
	glVertex2d(cx+width, cy+width);
	glVertex2d(cx+width, cy-width);

	glEnd();
}

//pt should be desired center of the square
void DrawRect(XnPoint3D pt, double width)
{
	glBegin(GL_QUADS);
	
	glVertex2d(pt.X-width, pt.Y-width);
	glVertex2d(pt.X-width, pt.Y+width);
	glVertex2d(pt.X+width, pt.Y+width);
	glVertex2d(pt.X+width, pt.Y-width);

	glEnd();
}



//-------------------------------------------------------------------------
//  Draws a string at the specified coordinates.
//    From http://mycodelog.com/2010/03/23/printw/
//-------------------------------------------------------------------------
void printw (float x, float y, float z, char* format, ...)
{
    va_list args;   //  Variable argument list
    int len;        // String length
    int i;          //  Iterator
    char * text;    // Text

	GLvoid *font_style = GLUT_BITMAP_TIMES_ROMAN_24;

    //  Initialize a variable argument list
    va_start(args, format);

    //  Return the number of characters in the string referenced the list of arguments.
    // _vscprintf doesn't count terminating '\0' (that's why +1)
    len = _vscprintf(format, args) + 1;

    //  Allocate memory for a string of the specified size
    text = (char *)malloc(len * sizeof(char));

    //  Write formatted output using a pointer to the list of arguments
    vsprintf_s(text, len, format, args);

    //  End using variable argument list
    va_end(args);

    //  Specify the raster position for pixel operations.
    glRasterPos3f (x, y, z);

    //  Draw the characters one by one
    for (i = 0; text[i] != '\0'; i++)
    glutBitmapCharacter(font_style, text[i]);

    //  Free the allocated memory for the string
    free(text);
}


//-------------------------------------------------------------------------
//  Draws a string at the specified coordinates.
//    From http://mycodelog.com/2010/03/23/printw/
//-------------------------------------------------------------------------
void printw (float x, float y, float z, std::string formatStr, ...)
{
    va_list args;   //  Variable argument list
    int len;        // String length
    int i;          //  Iterator
    char * text;    // Text

	GLvoid *font_style = GLUT_BITMAP_TIMES_ROMAN_24;

	char* format = new char[formatStr.size() + 1];
	std::strcpy(format, formatStr.c_str());

    //  Initialize a variable argument list
    va_start(args, format);

    //  Return the number of characters in the string referenced the list of arguments.
    // _vscprintf doesn't count terminating '\0' (that's why +1)
    len = _vscprintf(format, args) + 1;

    //  Allocate memory for a string of the specified size
    text = (char *)malloc(len * sizeof(char));

    //  Write formatted output using a pointer to the list of arguments
    vsprintf_s(text, len, format, args);

    //  End using variable argument list
    va_end(args);

    //  Specify the raster position for pixel operations.
    glRasterPos3f (x, y, z);

    //  Draw the characters one by one
    for (i = 0; text[i] != '\0'; i++)
    glutBitmapCharacter(font_style, text[i]);

    //  Free the allocated memory for the string
    free(text);
}


std::string ftos(float st)
{
	std::stringstream s;
	s.precision(2);
	s << st;
	return s.str();	
}

std::string itos(int i)
{
	std::stringstream s;
	s << i;
	return s.str();	
}

const char* ftoa(float st)
{
	std::stringstream s;
	s.precision(2);
	s << st;
	return s.str().c_str();
}


float mid(float a, float b)
{
	if (a > b)
		return a - (a-b)/2;
	else
		return b - (b-a)/2;
}


float lineDist(XnVector3D Pt, XnVector3D first, XnVector3D last)
{
	XnVector3D midpt;
	midpt.X = mid(last.X, first.X);
	midpt.Y = mid(last.Y, first.Y);
	midpt.Z = mid(last.Z, first.Z);

	return dist(Pt, midpt);
}



float dist(XnVector3D a, XnVector3D b)
{
	return sqrt(powf((a.X - b.X),2.f) + powf((a.Y - b.Y),2.f) + powf((a.Z - b.Z),2.f));
}

float dist2D(float x1, float y1, float x2, float y2)
{
	return sqrt(powf((x2-x1),2.f) + powf((y2-y1),2.f));
}

//For use in CSkeleton, generally
float newChord(Vector3df pos, int wx, int wy)
{
	//wx, wy are the window x/y max... probably only need the x, but w/e 

	//The x,z of the center of different chord types... ex: I, IV, V...
	float x1 = wx/2, z1=1900; //hard coding z for now
	float x2 = wx*23/32, z2=2200;
	float x3 = wx*11/16, z3=2600;
	float x4 = wx/2, z4=2800;
	float x5 = wx*5/16, z5=2600;
	float x6 = wx*9/32, z6=2200;

	float midX = wx/2, midZ = 2350; //best guess for z, for now

	std::vector<float> ds; //distances from key points
	ds.push_back(dist2D(pos.x(), pos.z(), x1, z1));
	ds.push_back(dist2D(pos.x(), pos.z(), x2, z2));
	ds.push_back(dist2D(pos.x(), pos.z(), x3, z3));
	ds.push_back(dist2D(pos.x(), pos.z(), x4, z4));
	ds.push_back(dist2D(pos.x(), pos.z(), x5, z5));
	ds.push_back(dist2D(pos.x(), pos.z(), x6, z6));
	ds.push_back(dist2D(pos.x(), pos.z(), midX, midZ));

	int best = ds[0];
	int b = 0;
	for (int i=1; i<(int)ds.size(); i++) 
	{
		if (ds[i] < best)
		{
			best = ds[i];
			b = i;
		}
	}

	//If return 7, it's center, maintain previous
	return (float)b + 1.f; //add one so range is 1-7... TODO: Consider only eval change if within a certain distance of those points
}


//Long, could be so much nicer looking... but hey, that's life. And I'm tired. 
//And besides, it's deprecated now.
std::string lookupKey(Key root, int prog)
{
	prog--;
	switch (root) 
	{
	case A: {
		std::string a[] = {"AMAJ", "BMIN", "DBMIN", "DMAJ", "EMAJ", "GBMIN"};
		return a[prog]; }
	case B: {
		std::string a[] = {"BMAJ", "DBMIN", "EBMIN", "EMAJ", "GBMAJ", "ABMIN"};
		return a[prog]; }
	case C: {
		std::string a[] = {"CMAJ", "DMIN", "EMIN", "FMAJ", "GMAJ", "AMIN"};
		return a[prog]; }
	case D: {
		std::string a[] = {"DMAJ", "EMIN", "GBMIN", "GMAJ", "AMAJ", "BMIN"};
		return a[prog]; }
	case E: {
		std::string a[] = {"EMAJ", "GBMIN", "ABMIN", "AMAJ", "BMAJ", "DBMIN"};
		return a[prog]; }
	case F: {
		std::string a[] = {"FMAJ", "GMIN", "AMIN", "BBMAJ", "CMAJ", "DMIN"};
		return a[prog]; }
	case G: {
		std::string a[] = {"GMAJ", "AMIN", "BMIN", "CMAJ", "DMAJ", "EMIN"};
		return a[prog]; }
	case Ab: {
		std::string a[] = {"ABMAJ", "BBMIN", "CMIN", "DBMAJ", "EBMAJ", "FMIN"};
		return a[prog]; }
	case Bb: {
		std::string a[] = {"BBMAJ", "CMIN", "DMIN", "EBMAJ", "FMAJ", "GMIN"};
		return a[prog]; }
	case Db: {
		std::string a[] = {"DBMAJ", "EBMIN", "FMIN", "GBMAJ", "ABMAJ", "BBMIN"};
		return a[prog]; }
	case Eb: {
		std::string a[] = {"EBMAJ", "FMIN", "GMIN", "ABMAJ", "BBMAJ", "CMIN"};
		return a[prog]; }
	case Gb: {
		std::string a[] = {"GBMAJ", "ABMIN", "BBMIN", "BMAJ", "DBMAJ", "EBMIN"};
		return a[prog]; }
	default: {
		std::string a[] = {"CMAJ", "DMIN", "EMIN", "FMAJ", "GMAJ", "AMIN"};
		return a[prog]; }
	}

	//return "CMAJ";
}


float minmax(float min, float value, float max)
{
	float toRet = min(max, value); //Don't want value to be greater than the max
	return max(toRet, min); //Don't want it to be less than the min
}


//Return the number scaled to between 0 and 1
float scale(float min, float value, float max)
{
	return (value - min)/(max - min);
}

float newScale(float newmin, float newmax, float oldmin, float oldmax, float value)
{
	float prop = scale(oldmin, value, oldmax);
	float dif = newmax - newmin;
	return (dif * prop) + newmin;
}


//Adjust var so that it's closer to add.
void adjust(float &var, float add)
{
	// oldVar ---- newVar ---- 1/2 ---- 3/4 ---- add
	var = mid(var, mid(var, add)); //Bumps it a quarter of the way closer to add
}


Key lower(Key k)
{
	int newK = (int)k - 1;
	if (newK < 0) newK = 11; //Wrap-around
	return static_cast<Key>(newK);
}

Key raise(Key k)
{
	int newK = (int)k + 1;
	if (newK > 11) newK = 0; //Wrap-around
	return static_cast<Key>(newK);
}

void AdjustMode(Key* chord, std::string mode)
{
	//----------------Minor Keys---------------
	if (mode == "Dorian") //Flat 3 and 7
	{
		chord[2] = lower(chord[2]);
		chord[6] = lower(chord[6]);
		return;
	}
	if (mode == "Phrygian") //Flat 2, 3, 6, 7
	{
		chord[1] = lower(chord[1]);
		chord[2] = lower(chord[2]);
		chord[5] = lower(chord[5]);
		chord[6] = lower(chord[6]);
		return;
	}
	if (mode == "Aeolian") //Flat 3, 6, 7
	{
		chord[2] = lower(chord[2]);
		chord[5] = lower(chord[5]);
		chord[6] = lower(chord[6]);
		return;
	}


	//--------------Major Keys---------------
	if (mode == "Major") //Default is major, don't change
	{
		return;
	}
	if (mode == "Lydian") //Sharp 4
	{
		chord[3] = raise(chord[3]);
		return;
	}


	//Don't worry about Mixolydian and Locrian, for now... not minor or major.

}



void fillChord(Key* chord, Key* newC)
{
	for (int i=0; i<7; i++)
	{
		chord[i] = newC[i];
	}
}


//void lookupProg(Key root, Key newRoot, std::string mode, std::string newMode, float ch)

//Could be more efficient... deal with it.
//Default is normal major.
Key lookupAllKeys(Key root, std::string mode, Key* chord, float ch)
{
	Key newRoot;
	std::string newMode;

	if (ch == 0) {
		newRoot = root;
		newMode = mode;
	}
	else {
		lookupProg(root, &newRoot, mode, &newMode, ch);
	}

	switch (newRoot) 
	{
	case A: {
		Key k[] = {A, B, Db, D, E, Gb, Ab};
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case B: {
		Key k[] = {B, Db, Eb, E, Gb, Ab, Bb}; 
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case C: {
		Key k[] = {C, D, E, F, G, A, B}; 
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case D: {
		Key k[] = {D, E, Gb, G, A, B, Db}; 
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case E: {
		Key k[] = {E, Gb, Ab, A, B, Db, Eb}; 
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case F: {
		Key k[] = {F, G, A, Bb, C, D, E}; 
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case G: {
		Key k[] = {G, A, B, C, D, E, Gb}; 
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case Ab: {
		Key k[] = {Ab, Bb, C, Db, Eb, F, G}; 
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case Bb: {
		Key k[] = {Bb, C, D, Eb, F, G, A};  
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case Db: {
		Key k[] = {Db, Eb, F, Gb, Ab, Bb, C};  
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case Eb: {
		Key k[] = {Eb, F, G, Ab, Bb, C, D}; 
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	case Gb: {
		Key k[] = {Gb, Ab, Bb, B, Db, Eb, F}; 
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	default: {
		Key k[] = {C, D, E, F, G, A, B}; 
		AdjustMode(k, newMode);
		fillChord(chord, k); }
	}
	return newRoot;
}


//Determines if two notes are exactly equal. 
//Best to do this when all the attacks/decays are equal.
bool NoteEqual(note A, note B)
{
	if (A.A == B.A && A.D == B.D && A.k == B.k && A.Oct == B.Oct)
		return true;
	return false;
}

std::string roman(int c)
{
	switch(c) {
	case 1:
		return "I";
	case 2:
		return "II";
	case 3:
		return "III";
	case 4:
		return "IV";
	case 5:
		return "V";
	case 6:
		return "VI";
	default:
		return "";
	}
}

/*
W	Whole Note
H	Half Note
Q	Quarter Note
I	1/8th Note
S	1/16th Note
*/
//Might this have been easier just saying if i == 5, return "QS"? ...sigh
std::string addWait(int i, bool rest)
{
	std::string s = "";
	if (rest && i!=0) s += "R";

	switch(i) {
	case 0:
		return s + "";//Nothing
	case 1:
		return s + "S";
	case 2:
		return s + "I";
	case 3:
		return s + "IS";
	case 4:
		return s + "Q";
	case 5:
		return s + "QS";
	case 6:
		return s + "QI";
	case 7:
		return s + "QIS";
	case 8:
		return s + "H";
	case 9:
		return s + "HS";
	case 10:
		return s + "HI";
	case 11:
		return s + "HIS";
	case 12:
		return s + "HQ";
	case 13:
		return s + "HQS";
	case 14:
		return s + "HQI";
	case 15:
		return s + "HQIS";
	default:
		return s + "";//nothing?
	}
}

std::string keyToString(Key k)
{
	switch(k) {
	case A:
		return "A";
	case B:
		return "B";
	case C:
		return "C";
	case D:
		return "D";
	case E:
		return "E";
	case F:
		return "F";
	case G:
		return "G";
	case Ab:
		return "Ab";
	case Bb:
		return "Bb";
	case Db:
		return "Db";
	case Eb:
		return "Eb";
	case Gb:
		return "Gb";
	default:
		return "C"; //Nothing? C?
	}
}

std::string noteToString(note n)
{
	std::string s = keyToString(n.k);
	s += itos(minmax(0.,n.Oct,10.));
	s += addWait(n.Dr, false);//"H"; //DURATION
	s += "A" + itos(n.A);
	s += "D" + itos(n.D);

	return s;
}



//Simply returns the radians of the angle... Should check for range between 0 and pi
float angle(XnVector3D mid, XnVector3D first, XnVector3D last)
{
	/*	
	Vector3 first = Vector3(1, 0, 0);
	Vector3 second = Vector3(0, 1, 0);

	first.Unitize(); // AKA Normalize
	second.Unitize();

	float dotProduct = first.Dot(second);   // Calculate dot product of two unit length vectors

	float angle = acos(dotProduct);   // acos of dot product gives angle
	*/
	
	Vector3df A(first.X - mid.X, first.Y - mid.Y, first.Z - mid.Z);
	Vector3df B(last.X - mid.X, last.Y - mid.Y, last.Z - mid.Z);
	A = A.normalize();
	B = B.normalize();

	float DP = A.dot(B);

	return acos(DP);
}



//-------------------------------------------------------------------------
//  Draws an oval centered at (x_center, y_center)
//
// The oval is bound inside a rectangle whose width is w
// and height is h.
//
// n represents the number of line segments used to draw the oval.
//-------------------------------------------------------------------------
void DrawOval (float x_center, float y_center, float w, float h, int n, bool fill)
{
    float theta, angle_increment;
    float x, y;
    if (n <= 0)
        n = 1;
    angle_increment = 3.14159265 / n;
    glPushMatrix ();
 
    //  center the oval at x_center, y_center
    glTranslatef (x_center, y_center, 0);
    //  draw the oval using line segments
	if (fill)
		glBegin(GL_TRIANGLE_FAN);
	else
		glBegin (GL_LINE_LOOP);
 
    for (theta = 0.0f; theta < 3.14159265; theta += angle_increment)
    {
        x = w/2 * cos (theta);
        y = h/2 * sin (theta);
  
        //  Color the vertices!
        //if (color[currentShape])
            //glColor3f (x, y, x*y);
        glVertex2f (x, y);
    }
    glEnd ();
    glPopMatrix ();
}


void DrawCircle(float cx, float cy, float r, int num_segments) 
{ 
	float theta = 2 * (float)3.1415926 / float(num_segments); 
	float c = cosf(theta);//precalculate the sine and cosine
	float s = sinf(theta);
	float t;

	/*float x = r;//we start at angle = 0 
	float y = 0; */
	float start = -1.83259571f;//radians
	float x = cosf(start) * r;// + cx;
	float y = sinf(start) * r;// + cy;

	glBegin(GL_LINE_LOOP); 
	for(int ii = 0; ii < num_segments; ii++) 
	{ 
		glVertex3f(x + cx, y + cy, 0.f);//output vertex 
		
        
		//apply the rotation matrix
		t = x;
		x = c * x - s * y;
		y = s * t + c * y;

		//Update color here, if changing...
	} 
	glEnd(); 
}

void DrawRomNums(float cx, float cy, float r)
{
	//Draws the chord names around the circle
	float theta = 2 * (float)3.1415926 / 6.f; 
	float c = cosf(theta);//precalculate the sine and cosine
	float s = sinf(theta);
	float t;

	/*float x = r;//we start at angle = 0 
	float y = 0; */
	float start = -1.57079633f;//degrees
	float x = cosf(start) * r;// + cx;
	float y = sinf(start) * r;// + cy;

	for(int ii = 0; ii < 6.f; ii++) 
	{ 
		//glVertex3f(x + cx, y + cy, 0.f);//output vertex 
		printw(x + (cx - 20.f), y + (cy + 20.f), 0.f, roman(ii+1));
        
		//apply the rotation matrix
		t = x;
		x = c * x - s * y;
		y = s * t + c * y;

	}  
}